import cv2
import numpy as np
from matplotlib import pyplot as plt
def trans(bef, M):
	
	bef = np.append(bef, [1])#补齐位数，应用变换矩阵
	bef = bef.reshape(3,1)#变换结构 3行1列
	befm = np.matrix(bef)

	#src(x,y),计算dst(x1,y1)的公式：
	#dst(x1,y1) = ((M11x+M12y+M13)/(M31x+M32y+M33),(M21x+M22y+M23)/(M31x+M32y+M33))
	#矩阵运算 M(3*3) befm(3*1)
	aftm = M * befm
	#变换后的坐标
	aftm = (aftm / aftm[2,0])[:2, 0]
	return np.array(aftm).reshape(1,2)

def main():
	# #主码流
	# img = cv2.imread('E:\\python\\opencv\\test.png')
	# #比例:110px=65cm=1块砖
	# scale = 110 / 1
	# #变换前4点坐标
	# pts1 = np.float32([[721,357],[883,387],[652,439],[830,478]])
	# #变换后4点坐标
	# pts2 = np.float32([[830,373],[830,483],[720,373],[720,483]])
	# #原图坐标
	# bef = np.float32([[591, 129]])
	# #原图坐标系原点
	# rad = np.float32([[591, 128]])
	#子码流
	img = cv2.imread('E:\\python\\opencv\\testzb.png')
	scale = 1
	pts1 = np.float32([[106.7,185],[168.85,206],[63.25,230],[129.25,261]])
	#pts2 = np.float32([[9905,10966],[10369,11472],[9318,11635],[9928,11915]]) / scale
	#pts2 = np.float32([[64.41,103],[71.31,103],[64.41,96.5],[71.31,96.5]])
	pts2 = np.float32([[330,164],[398,155],[328,75],[395,90]])
	#pts2 = np.float32([[330,155],[400,155],[330,90],[400,90]]) * 10
	bef = np.float32([[31,130]])
	#rad = np.float32([[162,52]])

	rows,cols,ch = img.shape
	#计算变换矩阵
	M = cv2.getPerspectiveTransform(pts1,pts2)
	#应用到图像
	#TODO:图像位置调整
	dst = cv2.warpPerspective(img,M,(cols,rows),cv2.INTER_LINEAR)
	src = img
	aft = trans(bef, M)
	#转换后坐标系原点
	#radt = trans(rad, M)
	radt = np.array([[0,0]])
	#坐标系中坐标
	#out = np.abs(aft - radt) / scale
	out = np.abs(aft - radt)
	print(aft, radt)
	font = cv2.FONT_HERSHEY_SIMPLEX
	#src = cv2.putText(img, out[0].tostring(),tuple(bef[0]),font,4,(255,0,255),25)
	src = cv2.circle(src, tuple(bef[0]), 10, (255, 0, 0), -1)
	src = cv2.putText(src, '%.2f,%.2f' % (out[0,0], out[0,1]),tuple(bef[0]),font,1,(255,0,255),1)
	dst = cv2.circle(dst, tuple(np.int32(aft[0])), 10, (255, 0, 0), -1)
	dst = cv2.putText(dst, '%.2f,%.2f' % (out[0,0], out[0,1]),tuple(np.int32(aft[0])),font,1,(255,0,255),1)
	# dst = cv2.putText(dst, '1',tuple(np.int32(pts2[0,0])),font,1,(255,0,255),0.5)
	# dst = cv2.putText(dst, '2',tuple(np.int32(pts2[0,1])),font,1,(255,0,255),0.5)
	# dst = cv2.putText(dst, '3',tuple(np.int32(pts2[0,2])),font,1,(255,0,255),0.5)
	# dst = cv2.putText(dst, '4',tuple(np.int32(pts2[0,3])),font,1,(255,0,255),0.5)

	cv2.imwrite('E:\\python\\opencv\\testzbout.png', dst)
	cv2.imshow('dst',dst)
	cv2.imshow('src',src)
	cv2.waitKey(0)

if __name__ == '__main__':
	main()
